# !/usr/bin/env python3
# -*- coding: utf-8 -*-
# @Time    : 2025/07/21 16:00
# @Author  : Yida Hao
# @File    : get_camera.py

from time import sleep
from rclpy.node import Node
from sensor_msgs.msg import Image
from interfaces.msg import CamImages
from interfaces.srv import GetImage

# TODO: 从 workspace 公共目录引入
CAMERA_PREFIX = "/camera/"

SRV_NAME = 'get_camera_service'

"""
Service to get camera images from the robot.
"""


class GetCameraService(Node):
    def __init__(self):
        super().__init__(f'{SRV_NAME}_srv')

        # Discover camera topics dynamically
        self.discover_camera_topics()

        self.current_camera_images = [Image() for _ in self.cam_names]

        self.init_camera_subscribers()

        while not all(self.current_camera_images):
            self.get_logger().info("waiting for connection to robot")
            sleep(1)

        self.srv = self.create_service(GetImage, SRV_NAME, self.srv_callback)

        self.get_logger().info("service ready")
    
    def discover_camera_topics(self):
        """Discover all topics with CAMERA_PREFIX"""
        self.get_logger().info(f"Discovering camera topics with prefix: {CAMERA_PREFIX}")
        
        # Wait a bit for publishers to be ready
        sleep(1)
        
        # Get all topics
        topic_names_and_types = self.get_topic_names_and_types()
        
        # Filter topics with CAMERA_PREFIX and Image type
        self.cam_names = []
        for topic_name, topic_types in topic_names_and_types:
            if topic_name.startswith(CAMERA_PREFIX) and 'sensor_msgs/msg/Image' in topic_types:
                # Extract camera name by removing prefix
                camera_name = topic_name[len(CAMERA_PREFIX):]
                self.cam_names.append(camera_name)
                self.get_logger().info(f"Found camera topic: {topic_name} -> {camera_name}")
        
        if not self.cam_names:
            self.get_logger().warn(f"No camera topics found with prefix {CAMERA_PREFIX}, waiting...")
            sleep(1)
            self.discover_camera_topics()  # Retry
    
    def srv_callback(self, request, response):
        self.get_logger().info("service called")
        response.cam_images.name = self.cam_names
        response.cam_images.image = self.current_camera_images
        return response

    def init_camera_subscribers(self):
        self.camera_subscribers = []
        for i, camera_name in enumerate(self.cam_names):
            topic_name = CAMERA_PREFIX + camera_name
            sub = self.create_subscription(
                Image,
                topic_name,
                lambda msg, idx=i: self.update_camera_image(idx, msg),
                10
            )
            self.camera_subscribers.append(sub)
            self.get_logger().info(f"Subscribed to {topic_name}")
    
    def update_camera_image(self, camera_index: int, msg: Image):
        self.current_camera_images[camera_index] = msg

class GetCameraClient(Node):
    def __init__(self):
        super().__init__(f'{SRV_NAME}_cli')
        self.cli = self.create_client(GetImage, SRV_NAME)

        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting ...')

        self.req = GetImage.Request()

    def send_request(self):
        future = self.cli.call_async(self.req)
        return future
